
// 计算v1如何连接v2后的能量值
// nodev是v1的某一邻域
//
static double my_compute_fitting_line_energy(Node* nodev,template_v* v1,template_v* v2,
    double r)
{
    double dir[3]={v2->point[0]-v1->point[0],
        v2->point[1]-v1->point[1],v2->point[2]-v1->point[2]};
    normalize(dir,3);
    double normal[3]={0};
     double sum=0;
    for(Node* nit=nodev;nit!=NULL;nit=(Node*)(nit->Next))
    {
        template_v* v=(template_v*)(nit->value);

        if(v==v1||v==v2){continue;}
        double dir1[3]={v->point[0]-v1->point[0],
        v->point[1]-v1->point[1],v->point[2]-v1->point[2]};
        double fac=inner_product(dir,dir1,3);

        if(fac<0){continue;}
        double temp_nor[3]={0};
        out_productn(dir,dir1,temp_nor);
        normalize(temp_nor,3);
        dir1[0]-=fac*dir[0];dir1[1]-=fac*dir[1];dir1[2]-=fac*dir[2];
        double e=inner_product(dir1,dir1,3)/(r*r);
        normal[0]+= temp_nor[0]/(e+1.0);
        normal[1]+= temp_nor[1]/(e+1.0);
        normal[2]+= temp_nor[2]/(e+1.0);
        sum++;
    }
    if(sum>1e-5)
    {
        return lb_norm(normal,3)/sum;

    }
    return 0;
}

// 这里kdtree是所有点集
// 需要v 的prop储存法向信息
// 似乎这个判断无法判断逆时针顺时针，限定条件仍显不足
//
static int tools_boundary_lines_is_correct_direction(template_v* v1,
    template_v* v2,KD_Node* kdtree,double r)
{
    // template_v* v=(template_v*)malloc(sizeof(template_v));
    // Vertex_init_(v);
    // v->point_size=3;
    // v->point=(double*)malloc(sizeof(double)*3);
    // v->point[0]=(v1->point[0]+v2->point[0])/2.0;v->point[1]=(v1->point[1]+v2->point[1])/2.0;v->point[2]=(v1->point[2]+v2->point[2])/2.0;
    double p[3]={(v1->point[0]+v2->point[0])*0.5,
        (v1->point[1]+v2->point[1])*0.5,
        (v1->point[2]+v2->point[2])*0.5};

    // Node* n=kd_tree_find_nearest_sphere(v,r,kdtree);
    Node* n=kd_tree_find_nearest_spherep(p,3,r,kdtree);

    double dir2[3]={ v1->point[0]-v2->point[0],
        v1->point[1]-v2->point[1],
        v1->point[2]-v2->point[2] };

    double normal[3]={0},normal1[3]={0};
    for(Node* nit=n;nit!=NULL;nit=(Node*)(nit->Next))
    {
        template_v* v3=(template_v*)(nit->value);
        double * n3=(double*)(v3->prop);
        // Normal_Boundary_Info*nbi=(Normal_Boundary_Info*)(v3->prop);
        // normal[0]+=nbi->normal[0]; normal[1]+=nbi->normal[1];normal[2]+=nbi->normal[2];
        normal[0]+=n3[0];normal[1]+=n3[1];normal[2]+=n3[2];

        if(v3==v1||v3==v2)
        {continue;}

        double dir1[3]={v3->point[0]-v2->point[0], v3->point[1]-v2->point[1],v3->point[2]-v2->point[2]};
        double dir[3]={0};
        out_productn(dir1,dir2,dir);
        normal1[0]+=dir[0]; normal1[1]+=dir[1];normal1[2]+=dir[2];
        // free(dir);
    }
    free_node(n);
    // free_Vertex(v);
    if(inner_product(normal1,normal,3)>0)
    {
        return 1;
    }else
    {
        return 0;
    }
    return 1;
}
static void tools_compute_adjacent_normal(Node* n,double r,double * re)
{
    double distance=0;
    Node* nit=n;
    template_v* v1=(template_v*)(nit->value);
    nit=(Node*)(nit->Next);
    // double * re=(double *)malloc(sizeof(double)*3);
    memset(re,0,sizeof(double)*3);

    while(nit!=NULL&&distance<1.5*r)
    {

        template_v* v2=(template_v*)(nit->value);
        double temp_dis=distance_of_two_points(v1->point,v2->point,3);
        double dir[3]={v1->point[0]-v2->point[0],v1->point[1]-v2->point[1],v1->point[2]-v2->point[2]};
        re[0]+=dir[0];re[1]+=dir[1];re[2]+=dir[2];

        distance+=temp_dis;
        v1=v2;
        nit=(Node*)(nit->Next);
    }
    // return re;
}
static template_v* increasing_boundaries_points_to_loop_find_one(
    Node**n,
    KD_Node*kdtree1,double r,template_v*v,
    Int_RB_Tree* tree,
    template_v* reverse_v )
{
    Node* n1=kd_tree_find_nearest_sphere(v,r,kdtree1);
    if(n1==NULL)
    {
        printf("n1 is null\n");
        return NULL;
    }
    double dir1[3]={0};
    tools_compute_adjacent_normal(*n,r,dir1);
    normalize(dir1,3);
        // printf("dir1 :%lf %lf %lf\n",dir1[0],dir1[1],dir1[2]);
    template_v* v3=NULL,*v4=NULL;
    double scores=0,fund_score=0;
    // printf("v id:%d revere_v id:%d\n",v->id,reverse_v->id);
        //double scores=distance_of_two_points(v->point,value[0]->point,3)/3.0*r;

    double angle_weight=1.2;

    for(Node* nit=n1;nit!=NULL;nit=(Node*)(nit->Next))
    {
        template_v* v2=(template_v*)(nit->value);
            // printf("v2 :%d\n",v2->id);
        double dir2[3]={v2->point[0]-v->point[0],v2->point[1]-v->point[1],v2->point[2]-v->point[2]};
        normalize(dir2,3);
        if(v2==v||tree->find(tree,v2->id)!=NULL||(v2!=reverse_v&&node_find(*n,v2)!=NULL))
        {
            continue;
        }
        if(inner_product(dir1,dir2,3)<-0.5)
        {
            continue;
        }
        if(v3==NULL)
        {
            v3=v2;v4=v2;

            scores=inner_product(dir1,dir2,3)*angle_weight+
                1-distance_of_two_points(v2->point,v->point,3)/(1.0*r);
                // my_compute_fitting_line_energy(n1,v,v2,r);

            (v2==reverse_v)?(scores+=0.15):(scores+=0.0);
            fund_score=scores;
            scores+=my_compute_fitting_line_energy(n1,v,v2,r);
            // printf("v2 id:%d ,point:%lf %lf %lf ,scores:%lf, score123:%lf %lf %lf\n",v2->id,
            //     v2->point[0],v2->point[1],v2->point[2],scores,
            //     inner_product(dir1,dir2,3),
            //     1-distance_of_two_points(v2->point,v->point,3)/(1.0*r),
            //     my_compute_fitting_line_energy(n1,v,v2,r));
        }
        else{
            double temp_scores=inner_product(dir1,dir2,3)*angle_weight+
                1-distance_of_two_points(v2->point,v->point,3)/(1.0*r);
                // my_compute_fitting_line_energy(n1,v,v2,r);

            (v2==reverse_v)?(temp_scores+=0.16):(temp_scores+=0.0);
            double temp_fund_score=temp_scores;
            temp_scores+=my_compute_fitting_line_energy(n1,v,v2,r);
            // printf("v2 id:%d ,point:%lf %lf %lf ,scores:%lf,score123:%lf %lf %lf \n",v2->id,
            //     v2->point[0],v2->point[1],v2->point[2],temp_scores,
            //     inner_product(dir1,dir2,3),
            //     1-distance_of_two_points(v2->point,v->point,3)/(1.0*r),
            //     my_compute_fitting_line_energy(n1,v,v2,r));
            if(temp_fund_score>fund_score)
            {
                fund_score=temp_fund_score;

                v4=v2;
            }
            if(temp_scores>scores)
            {
                scores=temp_scores;
                v3=v2;
            }
        }
    }

    template_v* re=NULL;
    v4==reverse_v?(re=v4):(re=v3);

    // printf("scores:%lf\n",scores);
    free_node(n1);
    return re;
}

/*
    @brief 将初始点集扩展为闭合环(loop)
    @param n 指向初始点集指针的指针，函数将在此点集基础上进行扩展
    @param tree 红黑树结构，用于额外排除不连接某些点(非多余参数)
    @param r 查找半径，控制点集扩展的范围
    @param kdtree1 KD树结构，存储查找备选点集
    @param fac 膨胀系数，控制扩展的幅度
    @return int 返回操作状态码(成功/失败等)
    @note 该函数通过初始点集开始生长，最终形成闭合环(loop)
*/

static int  increasing_boundaries_points_to_loop(Node**n,
    Int_RB_Tree*tree,double r ,KD_Node* kdtree1,double fac)
{
    Node* node_reversn=node_reverse(*n);
    template_v* reverse_v=(template_v*)(node_reversn->value);
    // printf("reverse v:%d\n",reverse_v->id);
    template_v* v=(template_v*)((*n)->value);
    while(v!=reverse_v)
    {
        // printf("v :%d\n",v->id);
        template_v* v3=increasing_boundaries_points_to_loop_find_one(
            n,
            kdtree1,r,v,
            tree,
            reverse_v );
        if(v3==NULL)
        {
            if(fac<=1.0)
            {break;}
            v3=increasing_boundaries_points_to_loop_find_one(
                n,
                kdtree1,fac*r,v,
                tree,
                reverse_v );
        }
        if(v3==NULL)
        {break;}
        // printf("v v3 dis:%lf\n",distance_of_two_points(v3->point,v->point,3));
        v=v3;
        *n=node_overlying(*n,v);
    }

    if(v!=reverse_v||node_size(*n)<=2)
    {
        printf("return 0\n");
        return 0;
    }
    return 1;
}


//从下面开始是偏应用的函数，需要根据情况修改
static template_v* my_get_max_energy_second_vertex(template_v*v,KD_Node* kdtree,double r,
    Int_RB_Tree* tree )
{
    template_v* re=NULL;
    Node* n1=kd_tree_find_nearest_sphere(v,r,kdtree);
    double score=0;
    for(Node* nit=n1;nit!=NULL;nit=(Node*)(nit->Next))
    {
        template_v* v1=(template_v*)(nit->value);
        if(v1==v||tree->find(tree,v1->id)!=NULL){continue;}
        double temp_score=1-distance_of_two_points(v1->point,v->point,3)/(1.0*r)+
            my_compute_fitting_line_energy(n1,v,v1,r);
        if(temp_score>score)
        {
            score=temp_score;
            re=v1;
        }
    }

    free_node(n1);
    return re;
}

// kdtree是整个点云的八叉树
// kdtree1是boundaries的八叉树
// 需要v->prop储存法向信息
// fac: 允许膨胀系数
// *****

Node* get_boundary_loop_from_one_vertex(KD_Node* kdtree,
    double r,Node* boundaries, KD_Node* kdtree1,template_v* v,
    double fac)
{
    Node* re=NULL;

    // KD_Node* kdnode=kd_tree_find_nearest_and_except_vs(v,kdtree1,&v,1);
    printf("second v\n");

    // if(distance_of_two_points(kdnode->value->point,v->point,3)>r)
    // {return re;}
    Int_RB_Tree* tree=(Int_RB_Tree*)malloc(sizeof(Int_RB_Tree));
    int_rb_tree_init(tree);

    template_v*second_v= my_get_max_energy_second_vertex(v,kdtree1,r,tree);
    if(second_v==NULL){return re;}
    //
    if(tools_boundary_lines_is_correct_direction(v,second_v,kdtree,r)==0)
    {
        re=node_overlying(re,second_v);
        re=node_overlying(re,v);
    }
    else
    {
        re=node_overlying(re,v);
        re=node_overlying(re,second_v);
    }

    if(increasing_boundaries_points_to_loop(&re,tree,r,kdtree1,fac)==0)
    {
        printf("shibai\n");
        // free_node(re);
        // re=NULL;
    }


    int_rb_tree_free(tree);

    return re;
}


// //@kdtree1:: kd tree of boundaries
// // @kdtree是整个网格的树
// 这个流程需要改
//

static template_v* my_get_max_densest_boundary_vertex(
    Node* boundaries,double r,KD_Node*kdtree1,
    Int_RB_Tree* tree)
{
    template_v* max_v=NULL;
    int max_num=0;

    for(Node* nit=boundaries;nit!=NULL;nit=(Node*)(nit->Next))
    {
        template_v* v=(template_v*)(nit->value);
        if(tree->find(tree,v->id)!=NULL)
        {continue;}

        Node* n1=kd_tree_find_nearest_sphere(v,0.5*r,kdtree1);

        int len=node_size(n1);
        if(len>max_num)
        {
            max_num=len;
            max_v=v;
        }
        free_node(n1);
    }

    return max_v;
}




static Node*  my_connect_one_boundary_points(Mesh*mesh,KD_Node* kdtree,
    double r,Node* boundaries,Int_RB_Tree* tree)
{
    double fac=1.25;
    Node* re=NULL;

    KD_Node* kdtree1=create_kd_tree_from_node(boundaries);
    template_v* max_v=my_get_max_densest_boundary_vertex(
        boundaries,r,kdtree1,tree);
    while(max_v!=NULL)
    {
        template_v*second_v= my_get_max_energy_second_vertex(max_v,
            kdtree1,r,tree);
        if(second_v!=NULL)
        {
            Node* n=NULL;
            if(tools_boundary_lines_is_correct_direction(max_v,second_v,kdtree,r)==0)
            {
                n=node_overlying(n,second_v);
                n=node_overlying(n,max_v);
            }
            else
            {
                n=node_overlying(n,max_v);
                n=node_overlying(n,second_v);
            }

            if(increasing_boundaries_points_to_loop(&n,tree,r,kdtree1,fac)==0)
            {
                printf("shibai\n");
                free_node(n);
                n=NULL;
            }
            else
            {

                Int_RB_Tree* tree1=(Int_RB_Tree*)malloc(sizeof(Int_RB_Tree));
                int_rb_tree_init(tree1);
                Node* nit1=n;
                while(nit1->Next!=NULL)
                {
                    template_v* v1=(template_v*)(nit1->value);
                    template_v* v2=(template_v*)(
                        ((Node*)(nit1->Next))->value);
                    tree->insert(tree,v1->id,v1);
                    tree->insert(tree,v2->id,v2);

                    template_v* vv1=(template_v*)(tree1->find(tree1,v1->id));
                    if(vv1==NULL)
                    {
                        vv1=mesh->create_vertexv(mesh,v1->point,3);
                    }
                    template_v* vv2=(template_v*)(tree1->find(tree1,v2->id));
                    if(vv2==NULL)
                    {
                        vv2=mesh->create_vertexv(mesh,v2->point,3);
                    }
                    vv1->prop=v1;
                    vv2->prop=v2;
                    //printf("create cell\n");
                    template_v*vs[2]={vv2,vv1};
                    mesh->create_cellv(mesh,vs,2);
                    tree1->insert(tree1,v1->id,vv1);tree1->insert(tree1,v2->id,vv2);
                    nit1=(Node*)(nit1->Next);
                }
                int_rb_tree_free(tree1);
                // tree->insert(tree,max_v->id,max_v);

                re=n;
                break;
                // re=node_overlying(re,n);
            }
        }

        tree->insert(tree,max_v->id,max_v);

        max_v=my_get_max_densest_boundary_vertex(
            boundaries,r,kdtree1,tree);
    }


    free_kdnode(kdtree1);

    return re;
}

//

Node* connect_boundary_points(Mesh*mesh,KD_Node* kdtree,
    double r,Node* boundaries,Int_RB_Tree* tree)
{
    double fac=1.25;
    //确保mesh是表示边界的
    mesh->dimension=1;mesh->simplex=1;

    Node* re=NULL;
    // printf("boundaries :%d\n",node_size(boundaries));
    Node* bound= node_copy(boundaries);
    Int_RB_Tree* tree2=(Int_RB_Tree*)malloc(sizeof(Int_RB_Tree));
    int_rb_tree_init(tree2);
    for(Node*nit=bound;nit!=NULL;nit=(Node*)(nit->Next))
    {
        template_v* v=(template_v*)(nit->value);
        tree2->insert(tree2,v->id,v);
    }
    Int_RB_Tree*tree1= int_rb_tree_copy(tree);

    Node* temp_n=my_connect_one_boundary_points(mesh,kdtree,
        r,bound,tree1);
    while(temp_n!=NULL)
    {
        re=node_overlying(re,temp_n);

        for(Node* nit=temp_n;nit!=NULL;nit=(Node*)(nit->Next))
        {
            template_v* v=(template_v*)(nit->value);
            tree2->erase(tree2,v->id);
        }
        free_node(bound);
        bound=NULL;
        for(auto it=tree2->begin(tree2);it.it!=NULL;it++)
        {
            bound=node_overlying(bound,it.second);
        }
        temp_n=my_connect_one_boundary_points(mesh,kdtree,
            r,bound,tree1);
    }
    int_rb_tree_free(tree1);
    free_node(bound);
    int_rb_tree_free(tree2);

    printf("mesh num c:%d\n",mesh->num_c(mesh));
    return re;

}
